home *** CD-ROM | disk | FTP | other *** search
/ Power Programmierung 2 / Power-Programmierung CD 2 (Tewi)(1994).iso / gnu / gnulib / sipp / sphigs / src / src.arj / MAT3FACT.C < prev    next >
Encoding:
C/C++ Source or Header  |  1993-10-12  |  8.9 KB  |  300 lines
  1. /* Copyright 1991, Brown Computer Graphics Group.  All Rights Reserved. */
  2.  
  3. #ifndef lint
  4. static char Version[] = 
  5.    "$Id: MAT3factor.c,v 1.2 1993/03/09 02:00:54 crb Exp $";
  6. #endif
  7.  
  8. /* -------------------------------------------------------------------------
  9.  * Routines to do the MAT3factor operation, which
  10.  * factors a matrix m:
  11.  *     m = r s r^ u t, where r^ means transpose of r, and r and u are
  12.  *  rotations, s is a scale, and t is a translation.
  13.  * 
  14.  *  It is based on the Jacobi method for diagonalizing real symmetric
  15.  *  matrices, taken from Linear Algebra, Wilkenson and Reinsch, Springer-Verlag
  16.  *  math series, Volume II, 1971, page 204.  Call number QA251W623.
  17.  *  In ALGOL!
  18.  * ------------------------------------------------------------------------- */
  19.  
  20. #include "mat3defs.h"
  21.  
  22. /* ------------------------ Static Routines  ------------------------------- */
  23.  
  24. /* -------------------------------------------------------------------------
  25.  * DESCR   :    Perform the jacobi factorization on an n x n symmetric matrix A
  26.  * producing eigenvectors of the matrix as well, if so requested by
  27.  * eivec flag. The eigenvalues are returned as the entries of the array d,
  28.  * and the eigenvectors are returned as the columns of the matrix v.
  29.  * The value rot indicates the number of jacobi rotations used in doing
  30.  * the computation. The returned factorization is passed through the 
  31.  * superdiagonal entries of the matrix A.
  32.  * 
  33.  * RETURNS :    void
  34.  *
  35.  * DETAILS :    
  36.  * ------------------------------------------------------------------------- */
  37.  
  38. /*
  39.  * Variable declarations from the original source:
  40.  *
  41.  * n    : order of matrix A
  42.  * eivec: true if eigenvectors are desired, false otherwise.
  43.  * a    : Array [1:n, 1:n] of numbers, assumed symmetric!
  44.  *
  45.  * a    : Superdiagonal elements of the original array a are destroyed.
  46.  *      Diagonal and subdiagonal elements are untouched.
  47.  * d    : Array [1:n] of eigenvalues of a.
  48.  * v    : Array [1:n, 1:n] containing (if eivec = TRUE), the eigenvectors of
  49.  *      a, with the kth column being the normalized eigenvector with
  50.  *      eigenvalue d[k].
  51.  * rot    : The number of jacobi rotations required to perform the operation.
  52.  */
  53.  
  54. static void
  55. MAT3_jacobi(
  56.    MAT3mat a,            /* Symmetric array */        
  57.    int       n,            /* Number of rows of a */    
  58.    int       eivec_flag,        /* Whether to compute eigenvectors */    
  59.    MAT3mat d,            /* Array of eigenvalues of a */
  60.    MAT3mat v,            /* Array of eigenvectors of a */        
  61.    int      *rot             /* Number of jacobi rotations used */    
  62.    )
  63. {
  64.    double sm, c, s,        /* Smallest entry, cosine, sine of theta */
  65.       t,  h, g,        /* tan of theta, two scrap values     */
  66.       tau, theta, thresh,    /* sine/(1+cos), angle for jacobi rotn,
  67.                  * threshold for doing rotation at all     */
  68.       *b, *z;        /* two arrays of n doubles         */
  69.    int     p, q, i, j;
  70.  
  71.    ALLOCN(b, double, n, "Array 'b' in MAT3_jacobi");
  72.    ALLOCN(z, double, n, "Array 'z' in MAT3_jacobi");
  73.  
  74.    MAT3identity(d);
  75.    if (eivec_flag == TRUE) MAT3identity(v);
  76.  
  77.    for (p = 0; p < n; p++) {
  78.       b[p] = d[p][p] = a[p][p];
  79.       z[p] = 0.0;
  80.    }
  81.    *rot = 0;
  82.  
  83.    for (i = 0; i < 50; i++) { /* why 50? I don't know--it's the way the
  84.                  * folks who wrote the algorithm did it */
  85.       sm = 0.0;
  86.       for (p = 0; p < n-1; p++) for (q = p+1; q < n; q++) sm += ABS(a[p][q]);
  87.  
  88.       if (sm == 0.0) goto out;
  89.  
  90.       thresh = (i < 3 ? (.2 * sm / (n * n)) : 0.0);
  91.  
  92.       for (p = 0; p < n-1; p++) for (q = p+1; q < n; q++) {
  93.  
  94.      g = 100.0 * ABS(a[p][q]);
  95.  
  96.      if (i > 3 && (ABS(d[p][p]) + g == ABS(d[p][p])) &&
  97.               (ABS(d[q][q]) + g == ABS(d[q][q])))
  98.         a[p][q] = 0.0;
  99.  
  100.      else if (ABS(a[p][q]) > thresh) {
  101.         h = d[q][q] - d[p][p];
  102.  
  103.         if (ABS(h) + g == ABS(h)) t = a[p][q] / h;
  104.         else {
  105.            theta = .5 * h / a[p][q];
  106.            t = 1.0 / (ABS(theta) + sqrt(1 + theta * theta));
  107.            if (theta < 0.0)  t = -t;
  108.         } /* end of computing tangent of rotation angle */
  109.  
  110.         c = 1.0 / sqrt(1.0 + t*t);
  111.         s = t * c;
  112.         tau = s / (1.0 + c);
  113.         h = t * a[p][q];
  114.         z[p]    -= h;
  115.         z[q]    += h;
  116.         d[p][p] -= h;
  117.         d[q][q] += h;
  118.         a[p][q] = 0.0;
  119.  
  120.         for (j = 0; j < p; j++) {
  121.            g = a[j][p];
  122.            h = a[j][q];
  123.            a[j][p] = g - s * (h + g * tau);
  124.            a[j][q] = h + s * (g - h * tau);
  125.         }
  126.  
  127.         for (j = p+1; j < q; j++) {
  128.            g = a[p][j];
  129.            h = a[j][q];
  130.            a[p][j] = g - s * (h + g * tau);
  131.            a[j][q] = h + s * (g - h * tau);
  132.         }
  133.  
  134.         for (j = q+1; j < n; j++) {
  135.            g = a[p][j];
  136.            h = a[q][j];
  137.            a[p][j] = g - s * (h + g * tau);
  138.            a[q][j] = h + s * (g - h * tau);
  139.         }
  140.  
  141.         if (eivec_flag) for (j = 0; j < n; j++) {
  142.            g = v[j][p];
  143.            h = v[j][q];
  144.            v[j][p] = g - s * (h + g * tau);
  145.            v[j][q] = h + s * (g - h * tau);
  146.         }
  147.      }
  148.      (*rot)++;
  149.       }
  150.       for (p = 0; p < n; p++){
  151.      d[p][p] = b[p] += z[p];
  152.      z[p] = 0;
  153.       }
  154.    }
  155. out:
  156.    
  157.    FREE(b);
  158.    FREE(z);
  159. }
  160.  
  161. /* ------------------------ Private Routines ------------------------------- */
  162.  
  163.  
  164. /* ------------------------ Public Routines  ------------------------------- */
  165. /*LINTLIBRARY*/
  166. /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
  167. /* -------------------------------------------------------------------------
  168.  * DESCR   :    Factors a matrix @m@ into a product  @m@m = @r@ @s@
  169.  *         @r@^@u@@t@, where @r@^ means transpose of @r@, and @r@ and
  170.  *         @u@ are rotations, @s@ is a scale, and @t@ is a translation.
  171.  *
  172.  * RETURNS :    FALSE if matrix @m@ does not have fourth column = (0,0,0,1) or
  173.  *         if matrix @m@ has determinant zero, TRUE otherwise. If return
  174.  *         value is FALSE, values of @r@, @s@, @t@, and @u@ are
  175.  *         meaningless. 
  176.  *
  177.  * DETAILS :    The factorization requires that m[0][3] = m[1][3] = m[2][3] = 0
  178.  *         and m[3][3] = 1.
  179.  * ------------------------------------------------------------------------- */
  180. BOOL
  181. MAT3factor(
  182.    MAT3mat m,            /* Source matrix to be factored  */
  183.    MAT3mat r,            /* Rotation for conjugation  */
  184.    MAT3mat s,            /* Scale in rotated coord system  */ 
  185.    MAT3mat u,            /* Rotation of conjugated matrix  */
  186.    MAT3mat t            /* Translation component  */
  187.    )
  188. {
  189.    double    det,        /* Determinant of matrix A    */
  190.         det_sign;    /* -1 if det < 0, 1 if det > 0    */
  191.    register int i;
  192.    int        junk;
  193.    MAT3mat    a, b, d, at, rt;
  194.  
  195.    /* (0) Make sure last row of m is as required. */
  196.    det = 1.0 - m[3][3];
  197.    if (! (MAT3_IS_ZERO_VEC(m[3]) && MAT3_IS_ZERO(det)))    return(FALSE);
  198.  
  199.    MAT3identity(r);    /* Adjust the values to something reasonable */
  200.    MAT3identity(s);
  201.    MAT3identity(u);
  202.    MAT3identity(t);
  203.  
  204.    /* (1) T gets last column of M. */
  205.    for (i = 0; i < 3; i++) t[i][3] = m[i][3];
  206.  
  207.    /* (2) A = M - T. */
  208.    MAT3copy(a, m);
  209.    for (i = 0; i < 3; i++) a[i][3] -= t[i][3];
  210.  
  211.    /* (3) Compute det A. If negative, set sign = -1, else sign = 1 */
  212.    det = MAT3determinant(a);
  213.    if (MAT3_IS_ZERO(det)) return(FALSE);
  214.    det_sign = (det < 0.0 ? -1.0 : 1.0);
  215.  
  216.    /* (4) B = A * A^  (here A^ means A transpose) */
  217.    MAT3transpose(at, a);
  218.    MAT3mult(b, a, at);
  219.  
  220.    MAT3_jacobi(b, 3, TRUE, d, r, &junk);
  221.  
  222.    MAT3transpose(rt, r);
  223.  
  224.    /* Compute s = sqrt(d), with sign. Set d = s-inverse */
  225.    MAT3copy(s, d);
  226.    for (i = 0; i < 3; i++)
  227.       d[i][i] = 1.0 / (s[i][i] = det_sign * sqrt(s[i][i]));
  228.  
  229.    /* (5) Compute U = R^ S! R A. */
  230.    MAT3mult(u, d, r);
  231.    MAT3mult(u, rt, u);
  232.    MAT3mult(u, a, u);
  233.  
  234.    return(TRUE);
  235. }
  236.  
  237.  
  238. /* -------------------------------------------------------------------------
  239.  * DESCR   :    Find the eigenvalues of the 3 x 3 part of a MAT3mat whose
  240.  *         determinant is nonzero and which is diagonalizable by the
  241.  *         Jacobi method.
  242.  *
  243.  * RETURNS :    TRUE if eigenvalues could be computed, FALSE otherwise.
  244.  *
  245.  * DETAILS :    Eigenvalues are returned in the array @values@. The sort is
  246.  *         numerical, from most negative to most positive.
  247.  * ------------------------------------------------------------------------- */
  248. BOOL
  249. MAT3eigen_values(
  250.    MAT3vec values, /* The eigenvalues of the second argument    */
  251.    MAT3mat mat       /* The matrix whose eigenvalues are computed */
  252.    )
  253. {
  254.    MAT3mat diag;   /* diagonalized matrix            */
  255.    MAT3mat e_vecs; /* the eigen vectors - not computed        */
  256.    int     rots;   /* number of rots needed to diagonalize    */
  257.    double  tmp;    /* temp swap var  && determinate        */
  258.  
  259.    /*
  260.     * check for imposible conditions
  261.     */
  262.    tmp = MAT3determinant(mat);
  263.    if( MAT3_IS_ZERO(tmp) ) 
  264.       return( FALSE );
  265.  
  266.    /*
  267.     * Use the Jacobi algorithm (from above) to diagonalize the 
  268.     * matrix. The eigen values are then the diagonal
  269.     */
  270.    MAT3_jacobi( mat, 3, FALSE, diag, e_vecs, &rots );
  271.  
  272.    /*
  273.     * copy the diag elements into the result vector
  274.     */
  275.    MAT3_SET_VEC( values, diag[0][0], diag[1][1], diag[2][2] );
  276.    
  277.    /*
  278.     * sort the values
  279.     */
  280.    if( values[0] > values[1] ) {
  281.       tmp = values[0];
  282.       values[0] = values[1];
  283.       values[1] = tmp;
  284.    }
  285.  
  286.    if( values[0] > values[2] ) {
  287.       /* rotate entries */
  288.       tmp = values[2];
  289.       values[2] = values[1];
  290.       values[1] = values[0];
  291.       values[0] = tmp;
  292.    } else if ( values[1] > values[2] ) {
  293.       /* swap last two */
  294.       tmp = values[1];
  295.       values[1] = values[2];
  296.       values[2] = tmp;      
  297.    }
  298.    return( TRUE );
  299. }
  300.